#include "initial_align.hpp"
#include "utils.hpp"

using namespace Eigen;
using namespace iNav;

InitialAlign::InitialAlign(std::vector<IMUData> static_imu_vec,
                           std::vector<GnssData> static_gnss_vec)
    : m_static_imu_vec(static_imu_vec),
      m_static_gnss_vec(static_gnss_vec),
      m_static_pos(0, 0, 0),
      m_static_acc(0, 0, 0) {
    /* initialize */
    // get average static data
    GetAverageStaticData();
    // get initial rotation
    Vector3d euler_angle(atan2(-m_static_acc[1], -m_static_acc[2]),
                         asin(m_static_acc[0] / m_static_acc.norm()), 0);

    m_initial_state.timestamp = m_static_imu_vec.back().timestamp;
    m_initial_state.pos = m_static_pos;
    m_initial_state.vel = Vector3d(0, 0, 0);
    m_initial_state.att = RAD_2_DEGREE * euler_angle;

    // init mechanization
    m_mechanization = new INS::INSMechanization(m_initial_state, m_static_imu_vec.back());
}

InitialAlign::~InitialAlign() {
    delete m_mechanization;
}

void InitialAlign::GetAverageStaticData() {
    for (auto gnss_data : m_static_gnss_vec)
        m_static_pos += 1.0 / m_static_gnss_vec.size() * gnss_data.pos;
    for (auto imu_data : m_static_imu_vec)
        m_static_acc += 1.0 / m_static_imu_vec.size() * imu_data.acc;
}

double InitialAlign::CalculateYaw(std::vector<IMUData> dynamic_imu_vec, GnssData gnss_data) {
    // mechanization
    NavData mechanization_data;
    for (auto imu_data : dynamic_imu_vec)
        mechanization_data = m_mechanization->MechanizationUpdate(imu_data);

    // calculate delta position vector
    Vector2d ins_delta_pos_vec = GetDeltaPositionVec(m_initial_state, mechanization_data);
    Vector2d gnss_delta_pos_vec = GetDeltaPositionVec(m_initial_state, gnss_data);

    double delta_phi = acos(gnss_delta_pos_vec.dot(ins_delta_pos_vec) /
                            (ins_delta_pos_vec.norm() * gnss_delta_pos_vec.norm()));

    double correct_yaw = mechanization_data.att[2] + RAD_2_DEGREE * delta_phi;
    return correct_yaw > 0 ? correct_yaw : correct_yaw + 360;
}

Vector2d InitialAlign::GetDeltaPositionVec(NavData initial_state, NavData final_state) {
    double delta_r_N, delta_r_E;
    Vector2d R = ComputeRmRn(DEGREE_2_RAD * initial_state.pos[0]);
    delta_r_N =
        DEGREE_2_RAD * (final_state.pos[0] - initial_state.pos[0]) * (R[0] + initial_state.pos[2]);
    delta_r_E =
        DEGREE_2_RAD * (final_state.pos[1] - initial_state.pos[1]) * (R[1] + initial_state.pos[2]);
    return Vector2d(delta_r_N, delta_r_E);
}

Vector2d InitialAlign::GetDeltaPositionVec(NavData initial_state, GnssData final_state) {
    double delta_r_N, delta_r_E;
    Vector2d R = ComputeRmRn(DEGREE_2_RAD * initial_state.pos[0]);
    delta_r_N =
        DEGREE_2_RAD * (final_state.pos[0] - initial_state.pos[0]) * (R[0] + initial_state.pos[2]);
    delta_r_E =
        DEGREE_2_RAD * (final_state.pos[1] - initial_state.pos[1]) * (R[1] + initial_state.pos[2]);
    return Vector2d(delta_r_N, delta_r_E);
}